/***************************************************************
 * PID control Class for segway
 * The functions of this class are used in "MinIMU9AHRS"
 **************************************************************/
 
 #include <PID_v1.h>
 
 double Setpoint, Input, Output;
 PID myPID(&Input, &Output, &Setpoint,0,0,0, DIRECT); // Create PID variable ready for initialization
 
 void initPID(double input, double setpoint, double Kp, double Ki, double Kd){
   Input = input;
   Setpoint = setpoint;
   
   updateTunings(Kp, Ki, Kd);
   myPID.SetControllerDirection(DIRECT);
   myPID.SetOutputLimits(-100,100);
 }
 
 void setModeAUT(){
  myPID.SetMode(AUTOMATIC); 
 }
 
 void setModeMAN(){
  myPID.SetMode(MANUAL); 
 }
 
 void updateTunings(double Kp, double Ki, double Kd){
   myPID.SetTunings(Kp,Ki,Kd);
 }
 
 void updateInput(double input){
  Input = input; 
 }
 
 double computePID(){
  myPID.Compute();
  return Output; 
 }


